#pragma once
#include "VisualData.h"
#include "ArRobot.h"

class MyRobot : public ArRobot {

    ///	posizione e orientazione iniziali del robot rispetto al sistema globale
    ArPose initialPose;

	///	matrice di rotazione e traslazione dal sistema globale al sistema robot
    cv::Mat_<double> globalToRobot;
	
	///	azione per raggiungere un obiettivo
    ArActionGoto gotoPoseAction;

	///	classe per gestire la telecamera
	VisualData visualData;

public:
    MyRobot(void);
    ~MyRobot(void);

    /**	Raggiunge una sequenza di obiettivi (punti sul piano del sistema globale)
     *	
     *	@param goals vettore degli obiettivi da raggiungere
     */
    void reachGoals(const std::vector<ArPose> &goals);

    /**	Raggiunge una sequenza di obiettivi (punti sul piano del sistema globale)
     *	e corregge la posizione rilevata dai dati odometrici utilizzando dati visuali
     *	
     *	@param goals vettore degli obiettivi da raggiungere
     */
    void reachGoalsAndCorrectPose(const std::vector<ArPose> &goals, int CORRECTION_INTERVAL);
};
